#include "glbase/glflat.h"
#include "glbase/glnavig.h"	

//#define DEBUG
#define N_TRIANG 4
#define MARK_SIZE 50.0f

double
		euler0[3]={ 0, 0, 0},
		Wnb0_b[3]={ 0, 0, 0},
		r0[3]={ 0, 0, 0},
		v0[3]={ 0, 0, 0},
		a0[3]={ 0, 0, -9.8};
int
	cMark=	0xffff8080,
	cFrame=	0xff505050,
	cBack=	0xffc0c0c0,
	cGrid=	0xffc0ffc0,
	object_colors[N_TRIANG]={INT_WGRAY,INT_WGRAY,INT_WHITE,INT_GRAY};
float
	vertices0[N_TRIANG][3][3]={
		{
			{ 0.0f,-0.1f, 0.0f},
			{ 0.0f, 0.0f,-0.5f},
			{ 1.0f, 0.0f, 0.0f},
		},
		{
			{ 0.0f, 0.1f, 0.0f},
			{ 1.0f, 0.0f, 0.0f},
			{ 0.0f, 0.0f,-0.5f},
		},
		{
			{ 1.0f, 0.0f, 0.0f},
			{ 0.0f, 0.5f, 0.0f},
			{ 0.0f,-0.5f, 0.0f},
		},
		{
			{ 1.0f, 0.0f, 0.0f},
			{ 0.0f,-0.5f,-0.1f},
			{ 0.0f, 0.5f,-0.1f},
		}},
	mark0[3]={ MARK_SIZE, 0.0f, 0.0f};

int i=0,j=0,k=0;
BOOL correct;
int kaccel=(int)pown(2,18),row,col,counter=0;
double dt;

double	
		euler[3],Wnb_b_sim[3],input_w[3],qi[4],Rbi[3][3],
		r_gps[3],v_gps[3],
		input_a[3],step_w=0.01f,
		u[3],ua;

float mark[3],vertices[N_TRIANG][3][3],normal[3];

BOOL
	initOK=FALSE,
	light=TRUE,
	fill=TRUE,
	pause=FALSE,
	pp=FALSE,
	kup=FALSE,
	kdp=FALSE,
	tabp=FALSE,
	lp=FALSE,
	retp=FALSE,
	sp=FALSE;
GLfloat
	z=-3.0f,
	LightAmbient[]= { 0.5f, 0.5f, 0.5f, 1.0f },
	LightDiffuse[]= { 1.0f, 1.0f, 1.0f, 1.0f },
	LightPosition[]= { 0.0f, 0.0f, 2.0f, 1.0f };



class SimIns:public FlatApplication
{
	INS ins;
	float zObject;
	BOOL initSim();
	void updateSim();
	void drawGeoPosition(
		float cx,float cy ,float cz,
		float lat, float lon, float alt,
		float x, float y, float z,
		float l, float t,
		int markColor = 0,
		int traceColor = 0,
		int frameColor = 0,
		int projectColor = 0,
		BOOL cercle = TRUE, int n = 32);
	void drawObject();

	BOOL create();
	void draw2D();
	void draw3D();
	BOOL glCreate();
	void update();
	BOOL listen(WORD id, WORD code, const void *info);
	
} simins;

BOOL SimIns::listen(WORD id, WORD code, const void *info)
{
	return FALSE;
}
BOOL SimIns::create ()				
{
	FlatApplication::create();
	name=_T("SimIns");

	format.precision=3;

	display.width=1024;
	display.height=768;
	display.bpp=32;

	return initSim();			
}
BOOL SimIns::glCreate ()
{
	glClearDepth(1.0f);
	glDepthFunc(GL_LEQUAL);
	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
	createLight(GL_LIGHT1,LightAmbient,LightDiffuse,LightPosition);
	setLight(light);
	FlatApplication::glCreate();
	return TRUE;												
}
void SimIns::update()
{
	dt=drawTime;
	counter++;
	BOOL *keys=keyboard.keys;
	updated=TRUE;

	if (keys [VK_ESCAPE])
	{
		terminate();
	}
	if (keys [VK_F1])
	{
		toggleFullscreen();
		keys [VK_F1] = FALSE;
	}
	if (keys ['L'])					// Is L Being Typed?
	{
		if(!lp)
		{
			lp=TRUE; // lp Becomes TRUE
			light=!light; // Toggle Light TRUE/FALSE
			setLight(light);
		}
	}
	else
	{
		lp = FALSE;
	}
	if (keys [VK_SPACE])					// Is Space Being Typed?
	{
		if(!sp)
		{
			sp=TRUE; // fp Becomes TRUE
			fill=!fill;
			fillPolygon(fill);
		}
	}
	else
	{
		sp=FALSE;
	}
	if (keys['P'])					// Is Space Being Typed?
	{
		if(!pp)
		{
			pp=TRUE; // fp Becomes TRUE
			pause=!pause;
		}
	}
	else
	{
		pp=FALSE;
	}
	if(pause)
		return;
	if (keys [VK_RETURN])			
	{
		if(!retp)
		{
			retp=TRUE; // fp Becomes TRUE
			initSim();
		}
	}
	else
	{
		retp=FALSE;
	}
	
	if (keys ['B'])	// Breaks		
	{
		initVector(Wnb_b_sim);
		initVector(ins.v_n);
		initVector(ins.v_n);
	}
	copyVector(a0,input_a);
	if (keys[VK_UP])
	{
		if (keys['K'])			
		{
			if(!kup)
			{
				kup=TRUE;
				if(kaccel<1<<20)
					kaccel<<=1;
				else
					kaccel=1;
			}
		}
		else
		{
			input_a[0]=kaccel;
		}
	}
	else
	{
		kup=FALSE;
	}
	if (keys[VK_DOWN])
	{
		if (keys['K'])			
		{
			if(!kdp)
			{
				kdp=TRUE;
				if(kaccel>1)
					kaccel>>=1;
				else
					kaccel=1<<20;
			}
		}
		else
		{
			input_a[0]=-kaccel;
		}
	}
	else
	{
		kdp=FALSE;
	}
	if(!keys['M'])
	{
		if (keys[VK_RIGHT])
			input_a[1]=+kaccel;
		else if (keys[VK_LEFT])
			input_a[1]=-kaccel;
	}
	if (keys[VK_NEXT]) 
	{
		input_a[2]=kaccel;
	}
	if (keys[VK_PRIOR]) 
	{
		input_a[2]=-kaccel;
	}
	if (keys[VK_OEM_MINUS]) 
	{
		zObject+=0.001f*zObject;
	}
	if (keys[VK_OEM_PLUS]) 
	{
		zObject-=0.001f*zObject;
	}
	if (keys['Q']) 
	{
		Wnb_b_sim[ROT_ROLL]-=step_w;
	}
	if (keys['E']) 
	{
		Wnb_b_sim[ROT_ROLL]+=step_w;
	}
	if (keys['W']) 
	{
		Wnb_b_sim[ROT_PITCH]-=step_w;
	}
	if (keys['S']) 
	{
		Wnb_b_sim[ROT_PITCH]+=step_w;
	}
	if (keys['D']) 
	{
		Wnb_b_sim[ROT_YAW]+=step_w;
	}
	if (keys['A']) 
	{
		Wnb_b_sim[ROT_YAW]-=step_w;
	}
	updateSim();
}
BOOL SimIns::initSim()
{
	int i,j,k;
	for(i=0;i<N_TRIANG;i++)
		for(j=0;j<3;j++)
			for(k=0;k<3;k++)
				vertices[i][j][k]=vertices0[i][j][k];
	copyVector(a0,input_a);
	copyVector(Wnb0_b,Wnb_b_sim);
	ins.inite(r0,v0,euler0,input_a,input_w);
	EulerToQuat(euler0,qi);
	zObject=z;
	return TRUE;
}
void SimIns::updateSim()
{
	ins.transRotation(Wnb_b_sim,input_w);
	ins.update(input_a,input_w,dt);
	getNextQuat(qi,input_w,dt); // real inertial attitude
	normQuat(qi);
	quatToRbn(qi,Rbi); // real inertial rotation
	RbnToEuler(ins.Rbn,euler);
	ua=(float)getQuatUnit(ins.q,u);
	mulVector(ins.Rbn,mark0,mark);
}
void SimIns::draw3D (void)
{
	glTranslatef( 0.0f, 0.0f, z);
	drawSphereIndicator(	0.9,1,0,
							(float)u[0],(float)u[1],(float)u[2],
							0.2f, 1.0f,
							INT_CYAN,cFrame);
	glLoadIdentity();
	glTranslatef(0.0f,0.0f,zObject);
	glPushMatrix();
	glRotatef(90.0f,0,0,1);
	glRotatef(-90.0f,1,0,0);
	//glScalef(0.5f,0.5f,0.5f);
	rotObject(Rbi,N_TRIANG,vertices0,vertices);
	drawObject();
	
	glPopMatrix();
	glRotatef(-70.0f,1,0,0);
	drawGeoPosition(
		0.0f,0.0f,0.0f,
		(float)ins.r_e[GEO_LAT],(float)ins.r_e[GEO_LON],(float)ins.r_e[GEO_ALT],
		(float)ins.re[0],(float)ins.re[1],(float)ins.re[2],
		1.0f,2.0f,INT_RED,INT_CYAN,cFrame,INT_WHITE);
}
void SimIns::draw2D()
{
	if(pause)
	{
		font.color=INT_RED;
		font.x= 0.5*(window.width-font.getTextWidth(_T("PAUSED")));
		font.y= 0.5*window.height;
		glPrint(&font,_T("PAUSED"));
	}
	font.color=INT_WHITE;
	font.x= window.width-210.0f;
	font.y= window.height-170.0f;
	glPrint(&font,_T("Map [m]:"));
	font.y+= 18;
	glPrint(&font,_T("u :%13.3lf"), ins.mapr[MAP_U]);
	font.y+= 18;
	glPrint(&font,_T("v :%13.3lf"), ins.mapr[MAP_V]);
	drawMapIndicator(
		float(font.x),float(font.y+18),
		200.0f,100.0f,
		(float)ins.map[MAP_U]+0.5f,(float)ins.map[MAP_V]+0.5f,
		2.0f,INT_BLUE,INT_WHITE,cFrame,cGrid,
		3,3);
	
	font.x=10;
	font.y=30;
	glPrint(&font,_T("S I M I N S"));
	font.color=INT_WGRAY;
	font.y+=18;
	glPrint(&font,_T("speed coef: %d"),kaccel);

	font.color=INT_GREEN;
	font.y+=40;
	glPrint(&font,_T("Position:"));
	font.y+=18;
	glPrint(&font,_T("x:%13.4f m"), ins.re[0]);
	font.y+=18;
	glPrint(&font,_T("y:%13.4f m"), ins.re[1]);
	font.y+=18;
	glPrint(&font,_T("z:%13.4f m"), ins.re[2]);
	font.color=INT_WGREEN;
	font.y+=25;
	glPrint(&font,_T("lat:% .6e rad"), ins.r_e[GEO_LAT]);
	font.y+=18;
	glPrint(&font,_T("lon:% .6e rad"), ins.r_e[GEO_LON]);
	font.y+=18;
	glPrint(&font,_T("alt:% .6e m"), ins.r_e[GEO_ALT]);

	font.color=0xFFC0FFC0;
	font.y+= 30;
	glPrint(&font,_T("Body A [m/s2]:"));
	font.y+= 18;
	glPrint(&font,_T("Ax:%+.4e"), ins.a[0]);
	font.y+= 18;
	glPrint(&font,_T("Ay:%+.4e"), ins.a[1]);
	font.y+= 18;
	glPrint(&font,_T("Az:%+.4e"), ins.a[2]);

	font.y+= 30;
	glPrint(&font,_T("Full Nav A [m/s2]:"));
	font.y+= 18;
	glPrint(&font,_T("Ax:%+.4e"), ins.vn_dot[0]);
	font.y+= 18;
	glPrint(&font,_T("Ay:%+.4e"), ins.vn_dot[1]);
	font.y+= 18;
	glPrint(&font,_T("Az:%+.4e"), ins.vn_dot[2]);
	
	font.color=0xFFFFFFB0;
	font.y+= 30;
	glPrint(&font,_T("Inert A [m/s2]:"));
	font.y+= 18;
	glPrint(&font,_T("Ax:%+.4e"), ins.aInert[0]);
	font.y+= 18;
	glPrint(&font,_T("Ay:%+.4e"), ins.aInert[1]);
	font.y+= 18;
	glPrint(&font,_T("Az:%+.4e"), ins.aInert[2]);

	font.color=0xFFB0FFFF;
	font.y+= 30;
	glPrint(&font,_T("Nav V [m/s]:"));
	font.y+= 18;
	glPrint(&font,_T("Vx:% .3e"), ins.v_n[0]);
	font.y+= 18;
	glPrint(&font,_T("Vy:% .3e"), ins.v_n[1]);
	font.y+= 18;
	glPrint(&font,_T("Vz:% .3e"), ins.v_n[2]);

	font.color=INT_ORANGE;
	font.y+= 30;
	glPrint(&font,_T("R:"));
	font.y+= 18;
	glPrint(&font,_T("% .3f % .3f % .3f"), ins.Rbn[0][0],ins.Rbn[0][1],ins.Rbn[0][2]);
	font.y+= 18;
	glPrint(&font,_T("% .3f % .3f % .3f"), ins.Rbn[1][0],ins.Rbn[1][1],ins.Rbn[1][2]);
	font.y+= 18;
	glPrint(&font,_T("% .3f % .3f % .3f"), ins.Rbn[2][0],ins.Rbn[2][1],ins.Rbn[2][2]);

	font.color=INT_CYAN;
	font.y= window.height-70;
	double y0=font.y;
	glPrint(&font,_T("Wib_b [rad/s]:"));
	font.y+= 18;
	glPrint(&font,_T("Wx:% .3e"), input_w[0]);
	font.y+= 18;
	glPrint(&font,_T("Wy:% .3e"), input_w[1]);
	font.y+= 18;
	glPrint(&font,_T("Wz:% .3e"), input_w[2]);

	font.color=INT_WBLUE;
	font.x+= 160;
	font.y=y0;
	glPrint(&font,_T("Win_n [rad/s]:"));
	font.y+= 18;
	glPrint(&font,_T("Wx:% .3e"), ins.Win_n[0]);
	font.y+= 18;
	glPrint(&font,_T("Wy:% .3e"), ins.Win_n[1]);
	font.y+= 18;
	glPrint(&font,_T("Wz:% .3e"), ins.Win_n[2]);

	font.color=INT_WGREEN;
	font.x+= 160;
	font.y=y0;
	glPrint(&font,_T("Wnb_b[rad/s]:"));
	font.y+=18;
	glPrint(&font,_T("roll :% .3e"), ins.Wnb_b[ROT_ROLL]);
	font.y+=18;
	glPrint(&font,_T("pitch:% .3e"), ins.Wnb_b[ROT_PITCH]);
	font.y+=18;
	glPrint(&font,_T("yaw  :% .3e"), ins.Wnb_b[ROT_YAW]);

	font.color=INT_WRED;
	font.x+= 190;
	font.y=y0;
	glPrint(&font,_T("Euler[deg]:"));
	font.y+= 18;
	glPrint(&font,_T("bank    :% 7.2f"), euler[0]*RAD_TO_DEG);
	font.y+= 18;
	glPrint(&font,_T("attitude:% 7.2f"), euler[1]*RAD_TO_DEG);
	font.y+= 18;
	glPrint(&font,_T("heading :% 7.2f"), euler[2]*RAD_TO_DEG);

	font.color=INT_WGRAY;
	font.x= window.width-125;
	font.y= 300;
	glPrint(&font,_T("XY:%6.1f"), atan2(mark[1],mark[0])*RAD_TO_DEG);
	drawCercleIndicator(	window.width-80, font.y-70,
							mark[0],mark[1],
							MARK_SIZE,2.0f,
							INT_RED,cFrame,cBack);
	font.y+= 140;
	glPrint(&font,_T("YZ:%6.1f"), atan2(mark[2],mark[1])*RAD_TO_DEG);
	drawCercleIndicator(	window.width-80, font.y-70,
							mark[1],mark[2],
							MARK_SIZE,2.0f,
							INT_RED,cFrame,cBack);
	font.y+= 140;
	glPrint(&font,_T("ZX:%6.1f"), atan2(mark[0],mark[2])*RAD_TO_DEG);
	drawCercleIndicator(	window.width-80, font.y-70,
							mark[2],mark[0],
							MARK_SIZE,2.0f,
							INT_RED,cFrame,cBack);

	font.color=INT_YELLOW;
	font.x= window.width-125;
	font.y= 20;
	glPrint(&font,_T("ua: %5.1f"), ua*RAD_TO_DEG);
	font.y+= 18;
	glPrint(&font,_T("ux:%6.3f"), u[0]);
	font.y+= 18;
	glPrint(&font,_T("uy:%6.3f"), u[1]);
	font.y+= 18;
	glPrint(&font,_T("uz:%6.3f"), u[2]);
	font.color=INT_DYELLOW;
	font.y+= 30;
	glPrint(&font,_T("q0:% .3f"), ins.q[0]);
	font.y+= 18;
	glPrint(&font,_T("q1:% .3f"), ins.q[1]);
	font.y+= 18;
	glPrint(&font,_T("q2:% .3f"), ins.q[2]);
	font.y+= 18;
	glPrint(&font,_T("q3:% .3f"), ins.q[3]);
}
void SimIns::drawGeoPosition(
		float cx,float cy ,float cz,
		float lat, float lon, float alt,
		float x, float y, float z,
		float l, float t,
		int markColor,
		int traceColor,
		int frameColor,
		int projectColor,
		BOOL cercle, int n)
{
	x*=float(l/A_EARTH);
	y*=float(l/A_EARTH);
	z*=float(l/A_EARTH);
	double
		clat=cos(lat),
		slat=sin(lat),
		clon=cos(lon),
		slon=sin(lon),
		R=getLength(x,y);
	
	glPushMatrix();
	glTranslatef(cx,cy,cz);

	glPushAttrib(GL_ENABLE_BIT);
	glEnable(GL_DEPTH_TEST);
	glDisable(GL_LIGHTING);
	glDisable(GL_BLEND);
	glDisable(GL_TEXTURE_2D);
	
	glLineWidth(t);
	if(frameColor&&cercle)
	{
		setColor(frameColor);
		drawCercle(0,0,l,n);
		glRotatef(90.0f,1,0,0);
		drawEllipse(0,0,l,l*K_EARTH,n);
		glRotatef(-90.0f,1,0,0);
	}
	glBegin(GL_LINES);
		if(frameColor)
		{
			setColor(frameColor);
			glVertex3f(-2*l, 0, 0);
			glVertex3f( 2*l, 0, 0);
			glVertex3f( 0,-2*l, 0);
			glVertex3f( 0, 2*l, 0);
			glVertex3f( 0, 0,-2*l);
			glVertex3f( 0, 0, 2*l);
		}
		if(projectColor)
		{
			setColor(projectColor);
			glVertex3f(x,y,0);
			glVertex3f(x,y,z);
			glVertex3f(x,0,0);
			glVertex3f(x,y,0);
			glVertex3f(0,y,0);
			glVertex3f(x,y,0);
		}
		markColor?setColor(markColor):setColor(INT_RED);
		glVertex3f(0,0,0);
		glVertex3f(x,y,z);
	glEnd();
	if(traceColor&&cercle)
	{
		setColor(traceColor);
		glPushMatrix();
			glRotatef(float(lon*RAD_TO_DEG),0,0,1);
			glRotatef(90.0f,1,0,0);
			drawEllipse(0,0,absval(l+l*alt/A_EARTH),absval(l*K_EARTH+l*alt/A_EARTH),n);
		glPopMatrix();
		glTranslatef(0,0,z);
		drawCercle(0,0,R,n);
	}
	glPopMatrix();
	glPopAttrib();
}
void SimIns::drawObject()
{
	glBegin(GL_TRIANGLES);
	for(int i=0;i<N_TRIANG;i++)
	{
		getNormalVector(vertices[i][0],vertices[i][1],vertices[i][2],normal);
		glNormal3fv(normal);
		setColor(object_colors[i]);
		glVertex3fv(vertices[i][0]);
		glVertex3fv(vertices[i][1]);
		glVertex3fv(vertices[i][2]);
	}
	glEnd();
}

int WINAPI _tWinMain(HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nCmdShow)
{
	return simins.main(hInstance,hPrevInstance,lpCmdLine,nCmdShow);
}/**/